package edu.wpi.first.wpilibj.swarm;

import edu.wpi.first.wpilibj.Utility;

public class TurnPD {
	Angle  target;
	Angle  botangle;
	double tgtrate;
	double output;
	long   time;
	double dt;
	double rotrate;
	double pwr_dtgt;
	private RobotMain    robomain;
	private LoggerServer logger;
	private boolean      needreset;
	
	static private final double Kp       = 1.5;
	static private final double Kd       = 0.2;
	static private final double turnrate = 5.25;
	static private final double turntol  = 0.01;
			
	TurnPD(double angle, RobotMain robotmain) {
		robomain  = robotmain;
		target    = new Angle();
		target.setAngle(angle);
		botangle  = new Angle();
		botangle.setAngle(angle);
		output    = 0.0;
		tgtrate   = 0.0;
		rotrate   = 0.0;
		pwr_dtgt  = 0.0;
		needreset = false;
		time      = Utility.getFPGATime();
		logger = new LoggerServer("botangle, targetangle, rotrate, tgtrate, output, target.getDiff(botangle)");
	}
	
	void reset() {
		needreset = true;
	}
	
	private void log() {
		logger.logString(botangle.getAngle() + ", " +
				target.getAngle() + ", " +
				rotrate + ", " +
				tgtrate + ", " +
				output + ", " +
				target.getDiff(botangle) + "\n");
	}
	
	private void revPD () {
		pwr_dtgt = (Kp * target.getDiff(botangle) + Kd * rotrate) / (Kp*dt + Kd);
	}
	
	double limabsval(double val, double max) {
		if (val > max) return max;
		if (val < -max) return -max;
		return val;
	}
	
	private void PD (double feedfwd) {
		target.addAngle(tgtrate * dt + limabsval(target.getDiff(botangle), turntol*dt));
		output  = Kp * target.getDiff(botangle) + Kd * (rotrate - tgtrate) + feedfwd;
	}
	
	double control(double rot_rate, Angle curangle, double input) {
		dt       = ((double) (Utility.getFPGATime() - time)) / 1000000.0;
		time    += dt * 1000000.0;
		botangle.setAngle(curangle.getAngle());
		if (robomain.isDisabled()) needreset = true;
		if (needreset) {
			target.setAngle(curangle.getAngle());
			needreset = false;
		}
		rotrate  = rot_rate;
		revPD();
		tgtrate  = -turnrate*input*(1.0-Math.abs(input)) + pwr_dtgt*Math.abs(input);
		PD(input);
		log();
		return output;
	}

	void resetTgt(Angle botangle) {
		target.setAngle(botangle.getAngle());
	}
}